//
// path_rml.c
//    convert path to Roland Modela .rml
//
// Neil Gershenfeld
// CBA MIT 9/6/10
//
// (c) Massachusetts Institute of Technology 2010
// Permission granted for experimental and personal use;
// license for commercial sale available from MIT.
//

//
// Modified by Blair Evans 2/17/2011 for MDX-40A mill
// changed from 100 steps per mm instead of 40 using mm_scale
//

#include "fab.h"

// #define mm_scale 40.0 // For Modela MDX-15/20
#define mm_scale 100.0 // For Modela MDX-40A


void fab_write_rml(struct fab_vars *v, char *output_file_name, float speed, int direction, float z_up) {
   //
   // write path to Roland Modela file
   //
	FILE *output_file;
   int i,x,y,z,iz_down,iz_up,nsegs=0,npts=0;
   float xscale,yscale,zscale,xoffset,yoffset,zoffset;
   output_file = fopen(output_file_name,"w");
   fprintf(output_file,"PA;PA;"); // plot absolute
   fprintf(output_file,"VS%.1f;!VZ%.1f;",speed,speed);
   xscale = mm_scale*v->dx/(v->nx-1.0); // 40/mm
   yscale = mm_scale*v->dy/(v->ny-1.0); // 40/mm
   if (v->nz > 1)
      zscale = mm_scale*v->dz/(v->nz-1.0); // 40/mm
   else
      zscale = 0;
   xoffset = mm_scale*v->xmin;
   yoffset = mm_scale*v->ymin;
   zoffset = mm_scale*v->zmin;
   iz_up = mm_scale*z_up;
   iz_down = zoffset;
   fprintf(output_file,"!PZ%d,%d;",iz_down,iz_up); // set z down, jog 
   fprintf(output_file,"!MC1;\n"); // turn motor on
   //
   // follow segments in reverse order (mill boundaries last)
   //
   v->path->segment = v->path->last;
   while (1) {
      if (direction == 0)
         //
         // conventional
         //
         v->path->segment->point = v->path->segment->last;
      else
         //
         // climb
         //
         v->path->segment->point = v->path->segment->first;
      x = xoffset + xscale * v->path->segment->point->first->value;
      y = yoffset + yscale * (v->ny - v->path->segment->point->first->next->value);
      //
      // move up to first point
      //
      fprintf(output_file,"PU%d,%d;",x,y);
      //
      // move down
      //
      if (v->path->dof == 2)
         fprintf(output_file,"PD%d,%d;",x,y);
      else if (v->path->dof == 3) {
         z = zoffset + zscale * v->path->segment->point->first->next->next->value;
         fprintf(output_file,"Z%d,%d,%d;",x,y,z);
         }
      else {
         printf("path_rml: path degrees of freedom must be 2 or 3\n");
         exit(-1);
         }
      nsegs += 1;
      while (1) {
         //
         // check if last point
         //
         if (direction == 0) {
            //
            // conventional
            //
            if (v->path->segment->point->previous == 0) {
               fprintf(output_file,"PU%d,%d;",x,y);
               break;
               }
            }
         else {
            //
            // climb
            //
            if (v->path->segment->point->next == 0) {
               fprintf(output_file,"PU%d,%d;",x,y);
               break;
               }
            }
         //
         // move to next point
         //
         if (direction == 0)
            //
            // conventional
            //
            v->path->segment->point = v->path->segment->point->previous;
         else
            //
            // climb
            //
            v->path->segment->point = v->path->segment->point->next;
         x = xoffset + xscale * v->path->segment->point->first->value;
         y = yoffset + yscale * (v->ny - v->path->segment->point->first->next->value);
         if (v->path->dof == 2)
            fprintf(output_file,"PD%d,%d;",x,y);
         else if (v->path->dof == 3) {
            z = zoffset + zscale * v->path->segment->point->first->next->next->value;
            fprintf(output_file,"Z%d,%d,%d;",x,y,z);
            }
         else {
            printf("path_rml: path degrees of freedom must be 2 or 3\n");
            exit(-1);
            }
         npts += 1;
         }
      //
      // check for previous segment
      //
      fprintf(output_file,"\n",x,y);
      if (v->path->segment->previous == 0)
         break;
      v->path->segment = v->path->segment->previous;
      }
   //
   // pad end of file with motor off commands for Modela buffering bug
   //
   for (i = 0; i < 1000; ++i)
      fprintf(output_file,"!MC0;");
   //
   // return to home
   //
   fprintf(output_file,"\nH;\n");
   fclose(output_file);
   printf("wrote %s\n",output_file_name);
   printf("   segments: %d, points: %d\n",nsegs,npts);
   }

main(int argc, char **argv) {
   //
   // local vars
   //
   struct fab_vars v = init_vars(&v);
   float speed,z_up;
   int direction;
   //
   // command line args
   //
   if (!((argc == 3) || (argc == 4) || (argc == 6) || (argc == 7) || (argc == 8) || (argc == 9))) {
      printf("command line: path_rml in.path out.rml [speed [xmin ymin [zmin [z_up [direction]]]]]\n");
      printf("   in.path = input path file\n");
      printf("   out.rml = output Roland Modela file\n");
      printf("   speed = cutting speed (optional, mm/s, default 4)\n");
      printf("   xmin = left position (optional, mm, default path value)\n");
      printf("   ymin = front position (optional, mm, default path value)\n");
      printf("   zmin = bottom position (optional, -mm, default path value)\n");
      printf("   z_up = toop up position (optional, mm, default 1)\n");
      printf("   direction = machining direction (optional, 0 conventional/1 climb, default 1)\n");
      exit(-1);
      }
   if (argc == 3) {
      speed = 4;
      direction = 1;
      z_up = 1;
      }
   else if (argc == 4) {
      sscanf(argv[3],"%f",&speed);
      z_up = 1;
      direction = 1;
      }
   else if (argc == 6) {
      sscanf(argv[3],"%f",&speed);
      z_up = 1;
      direction = 1;
      }
   else if (argc == 7) {
      sscanf(argv[3],"%f",&speed);
      z_up = 1;
      direction = 1;
      }
   else if (argc == 8) {
      sscanf(argv[3],"%f",&speed);
      sscanf(argv[7],"%f",&z_up);
      direction = 1;
      }
   else if (argc == 9) {
      sscanf(argv[3],"%f",&speed);
      sscanf(argv[7],"%f",&z_up);
      sscanf(argv[8],"%d",&direction);
      }
   //
   // read path
   //
   fab_read_path(&v,argv[1]);
   //
   // check origin
   //
   if ((argc == 6) || (argc == 7) || (argc == 8) || (argc == 9)) {
      sscanf(argv[4],"%lf",&v.xmin);
      sscanf(argv[5],"%lf",&v.ymin);
      }
   if ((argc == 7) || (argc == 8) || (argc == 9)) {
      sscanf(argv[6],"%lf",&v.zmin);
      }
   //
   // write .rml
   //
   fab_write_rml(&v,argv[2],speed,direction,z_up);
   }